﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

namespace KinectAdapter 
{
    public class ButlaStyle : KinectForkliftAdapter
    {
        public override event EventHandler StopIssued = null;
        public override event EventHandler MenuIssued = null;

        /// <summary>
        /// Hip position in Kinect's data needs to moved by this ammount
        /// to agree with what peaople percieve as their hip.
        /// </summary>
        private const double HIP_POSITION_CORRECTION = -0.1;

        /// <summary>
        /// If main steering is on then two values can be changed at the same time.
        /// Both will change in the same time if the smaller value is at least this
        /// fraction of the larger one.
        /// </summary>
        private const double STEERING_PROPORTION = 0.6;

        private const double X_DATA_SCALE = 2.0;
        private const double Y_DATA_SCALE = 1.5;
        private const double Z_DATA_SCALE = 4.0;
        private const double X_ZERO_RANGE = 0.15;
        private const double Y_ZERO_RANGE = 0.15;
        private const double Z_ZERO_RANGE = 0.15;

        /// <summary>
        /// Initiates the adapter.
        /// Start() methods needs to be called for it to start working.
        /// </summary>
        public ButlaStyle()
        {
            X = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = X_DATA_SCALE, ZeroRange = X_ZERO_RANGE };
            Y = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = Y_DATA_SCALE, ZeroRange = Y_ZERO_RANGE };
            Z = new KinectSlider() { Min = DEFAULT_MIN, Max = DEFAULT_MAX, Scale = Z_DATA_SCALE, ZeroRange = Z_ZERO_RANGE };
        }

        /// <summary>
        /// Handles the skeleton data and changes the values of control.
        /// Also detects gestures.
        /// </summary>
        /// <param name="skeleton">Skeleton data received.</param>
        protected override void GetSkeletonData(Skeleton skeleton)
        {          
            MainSteeringOn = CheckMainSteering(skeleton);
            SecondarySteeringOn = CheckSecondarySteering(skeleton);

            UpdateSteeringHandPositions(skeleton);
            
            if (!this.IsActive) return;  // do not detect any gestures or movement

            if (handPositions.Count >= MOVEMENT_POINTS) // try to move only when the minimum requirement for frames was met
            {
                if (MainSteeringOn)
                {
                    float xChange = GetMeanChange(handPositions.Select(val => val.X).ToArray());
                    float zChange = GetMeanChange(handPositions.Select(val => val.Z).ToArray()) * -1;   // reversed axis
                    double xChangeAbs = Math.Abs(xChange) * X_DATA_SCALE;
                    double zChangeAbs = Math.Abs(zChange) * Z_DATA_SCALE;

                    bool changeBoth = Math.Min(xChangeAbs, zChangeAbs) / Math.Max(xChangeAbs, zChangeAbs) > STEERING_PROPORTION;

                    if (changeBoth) // both moves are distinct, change both values
                    {
                        X.ChangeValueSnapToZero(xChange);
                        Z.ChangeValueSnapToZero(zChange);
                    }
                    else if (xChangeAbs > zChangeAbs)  // move along X axis is clearly dominant
                    {
                        X.ChangeValueSnapToZero(xChange);
                    }
                    else   // move along Z axis is clearly dominant
                    {
                        Z.ChangeValueSnapToZero(zChange);
                    }
                }
                if (SecondarySteeringOn)
                {
                    Y.ChangeValueSnapToZero(GetMeanChange(handPositions.Select(val => val.Y).ToArray()));
                }
            }

            if (CheckStop(skeleton))
            {
                if (CheckMenu(skeleton))    // menu gesture contains the stop gesture
                {
                    if (MenuIssued != null)
                    {
                        MenuIssued(this, null);
                    }
                    return;
                }
                else  // not menu, just stop
                {
                    if (StopIssued != null)
                    {
                        StopIssued(this, null);
                    }
                }
            }
        }

        /// <summary>
        /// Check if the main steering should be enabled.
        /// It will be when the left hand is below the left hip.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the main steering should be enabled.</returns>
        protected override bool CheckMainSteering(Skeleton skeleton)
        {
            return skeleton.Joints[JointType.HandLeft].Position.Y <= skeleton.Joints[JointType.HipLeft].Position.Y;
        }

        /// <summary>
        /// Check if the secondary steering should be enabled.
        /// It will be when the left hand is in space between the hips (actually past the left hip)
        /// and below shoulders.
        /// </summary>
        /// <param name="skeleton">Postions of operator's body parts.</param>
        /// <returns>True if the secondary steering should be enabled.</returns>
        protected override bool CheckSecondarySteering(Skeleton skeleton)
        {
            bool conditionPart1 = skeleton.Joints[JointType.HandLeft].Position.X >= skeleton.Joints[JointType.HipLeft].Position.X + HIP_POSITION_CORRECTION;
            bool conditionPart2 = skeleton.Joints[JointType.HandLeft].Position.Y < skeleton.Joints[JointType.ShoulderCenter].Position.Y;
            return conditionPart1 && conditionPart2;
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that all controls
        /// should be restored to their defaults (stop the forklift).
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the forklift should stop.</returns>
        protected override bool CheckStop(Skeleton skeleton)
        {
            SkeletonPoint left = skeleton.Joints[JointType.HandLeft].Position;
            SkeletonPoint right = skeleton.Joints[JointType.HandRight].Position;

            // both hands above shoulders
            if (left.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y
                && right.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y)
            {
                // hands touching
                if(OperatorSkeleton.GetDistance(left, right) <= JOINT_TOUCH_RANGE)
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// This should check if the operator made a gesture indicating that application
        /// should return to menu. HUD will still be working but will be hidden.
        /// Also, the forklift should stop.
        /// </summary>
        /// <param name="skeleton">Operator's skeleton.</param>
        /// <returns>True if the application should return to menu.</returns>
        protected override bool CheckMenu(Skeleton skeleton)
        {
            // first, the condition from stop needs to be met
            if (CheckStop(skeleton))
            {
                // old condition, sometimes unrealiable
                //double dist = OperatorSkeleton.GetDistance(skeleton.Joints[JointType.ElbowLeft].Position,
                //    skeleton.Joints[JointType.ElbowRight].Position);

                //// if the elbows are touching
                //if (dist <= JOINT_TOUCH_RANGE)
                //    return true;

                bool conditionPart1 = skeleton.Joints[JointType.ElbowLeft].Position.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y;
                bool conditionPart2 = skeleton.Joints[JointType.ElbowRight].Position.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y;
                return conditionPart1 && conditionPart2;
            }
            return false;
        }
    }
}
